/*
 * File:   brazo.c
 * Author: luis
 *
 * Created on 31 de agosto de 2014, 0:18
 */


#include <xc.h>
#include <xc.h>
#include<p18f2550.h>
#include<delays.h>
#include<stdint.h>
// CONFIG1L
#pragma config PLLDIV = 1       // PLL Prescaler Selection bits (No prescale (4 MHz oscillator input drives PLL directly))
#pragma config CPUDIV = OSC1_PLL2// System Clock Postscaler Selection bits ([Primary Oscillator Src: /1][96 MHz PLL Src: /2])
#pragma config USBDIV = 1       // USB Clock Selection bit (used in Full-Speed USB mode only; UCFG:FSEN = 1) (USB clock source comes directly from the primary oscillator block with no postscale)

// CONFIG1H
#pragma config FOSC = XT_XT     // Oscillator Selection bits (XT oscillator (XT))
#pragma config FCMEN = OFF      // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled)
#pragma config IESO = OFF       // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)

// CONFIG2L
#pragma config PWRT = OFF       // Power-up Timer Enable bit (PWRT disabled)
#pragma config BOR = ON         // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled))
#pragma config BORV = 3         // Brown-out Reset Voltage bits (Minimum setting)
#pragma config VREGEN = OFF     // USB Voltage Regulator Enable bit (USB voltage regulator disabled)

// CONFIG2H
#pragma config WDT = OFF        // Watchdog Timer Enable bit (WDT disabled (control is placed on the SWDTEN bit))
#pragma config WDTPS = 32768    // Watchdog Timer Postscale Select bits (1:32768)

// CONFIG3H
#pragma config CCP2MX = ON      // CCP2 MUX bit (CCP2 input/output is multiplexed with RC1)
#pragma config PBADEN = ON      // PORTB A/D Enable bit (PORTB<4:0> pins are configured as analog input channels on Reset)
#pragma config LPT1OSC = OFF    // Low-Power Timer 1 Oscillator Enable bit (Timer1 configured for higher power operation)
#pragma config MCLRE = ON       // MCLR Pin Enable bit (MCLR pin enabled; RE3 input pin disabled)

// CONFIG4L
#pragma config STVREN = ON      // Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset)
#pragma config LVP = OFF        // Single-Supply ICSP Enable bit (Single-Supply ICSP disabled)
#pragma config XINST = OFF      // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))

// CONFIG5L
#pragma config CP0 = OFF        // Code Protection bit (Block 0 (000800-001FFFh) is not code-protected)
#pragma config CP1 = OFF        // Code Protection bit (Block 1 (002000-003FFFh) is not code-protected)
#pragma config CP2 = OFF        // Code Protection bit (Block 2 (004000-005FFFh) is not code-protected)
#pragma config CP3 = OFF        // Code Protection bit (Block 3 (006000-007FFFh) is not code-protected)

// CONFIG5H
#pragma config CPB = OFF        // Boot Block Code Protection bit (Boot block (000000-0007FFh) is not code-protected)
#pragma config CPD = OFF        // Data EEPROM Code Protection bit (Data EEPROM is not code-protected)

// CONFIG6L
#pragma config WRT0 = OFF       // Write Protection bit (Block 0 (000800-001FFFh) is not write-protected)
#pragma config WRT1 = OFF       // Write Protection bit (Block 1 (002000-003FFFh) is not write-protected)
#pragma config WRT2 = OFF       // Write Protection bit (Block 2 (004000-005FFFh) is not write-protected)
#pragma config WRT3 = OFF       // Write Protection bit (Block 3 (006000-007FFFh) is not write-protected)

// CONFIG6H
#pragma config WRTC = OFF       // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) are not write-protected)
#pragma config WRTB = OFF       // Boot Block Write Protection bit (Boot block (000000-0007FFh) is not write-protected)
#pragma config WRTD = OFF       // Data EEPROM Write Protection bit (Data EEPROM is not write-protected)

// CONFIG7L
#pragma config EBTR0 = OFF      // Table Read Protection bit (Block 0 (000800-001FFFh) is not protected from table reads executed in other blocks)
#pragma config EBTR1 = OFF      // Table Read Protection bit (Block 1 (002000-003FFFh) is not protected from table reads executed in other blocks)
#pragma config EBTR2 = OFF      // Table Read Protection bit (Block 2 (004000-005FFFh) is not protected from table reads executed in other blocks)
#pragma config EBTR3 = OFF      // Table Read Protection bit (Block 3 (006000-007FFFh) is not protected from table reads executed in other blocks)

// CONFIG7H
#pragma config EBTRB = OFF      // Boot Block Table Read Protection bit (Boot block (000000-0007FFh) is not protected from table reads executed in other blocks)
#define  _XTAL_FREQ 4000000

void coger_el_objeto(){
 //PARA GOGER EL OBJETO
        PORTBbits.RB7=1;
        Delay10TCYx(210);
        PORTB=0;
        PORTBbits.RB6=1;
        Delay10TCYx(96);
        PORTB=0;
        PORTBbits.RB5=1;
        Delay10TCYx(115);
        PORTB=0;
        Delay10KTCYx(2);

}
void baja_la_mitad(){
//BAJA LA MITAD DEL BRAZO PARA COGER EL OBJETO
        PORTBbits.RB7=1;
        Delay10TCYx(210);
        PORTB=0;
        PORTBbits.RB6=1;
        Delay10TCYx(96);
        PORTB=0;
        PORTBbits.RB5=1;
        Delay10TCYx(50);
        PORTB=0;
        Delay10KTCYx(2);
    
}

void da_la_vuelta(){
  //DA LA VUELTA
        PORTBbits.RB7=1;
        Delay10TCYx(210);
        PORTB=0;
        PORTBbits.RB6=1;
        Delay10TCYx(96);
        PORTB=0;
        PORTBbits.RB5=1;
        Delay10TCYx(50);
        PORTB=0;
        PORTBbits.RB4=1;
        Delay10TCYx(210);
        PORTB=0;
        Delay10KTCYx(2);

}

void deja_el_objeto(){
//DEJA EL OBJETO
        PORTBbits.RB7=1;
        Delay10TCYx(210);
        PORTB=0;
        PORTBbits.RB6=1;
        Delay10TCYx(140);
        PORTB=0;
        PORTBbits.RB5=1;
        Delay10TCYx(115);
        PORTB=0;
        Delay10KTCYx(2);
    
}

void dalavuelta_de_regreso(){
 //DA LA VUELTA DE REGRESO
        PORTBbits.RB7=1;
        Delay10TCYx(210);
        PORTB=0;
        PORTBbits.RB6=1;
        Delay10TCYx(140);
        PORTB=0;
        PORTBbits.RB5=1;
        Delay10TCYx(115);
        PORTB=0;
        PORTBbits.RB4=1;
        Delay10TCYx(20);
        PORTB=0;
        Delay10KTCYx(2);
}


int o;
void main(void) {

    ADCON1=15;//convertir de analogas a digitales
    TRISB=0;//declarar las entradas de los motores
    while(1)//bucle infinito
    {

        for(int o=0;o<100;o++){
      coger_el_objeto();}

        for(int o=0;o<200;o++){
     baja_la_mitad();}

       for(int o=0;o<200;o++){
       da_la_vuelta();}

        for(int o=0;o<200;o++){
      deja_el_objeto();}

        for(int o=0;o<00;o++){
       dalavuelta_de_regreso();}

    }
}